#include "Car.hpp"

int currentSpeed = MAX_SPEED;

void BiDirectionalMotor::begin() {
    pinMode(posPin, OUTPUT);
    pinMode(resPin, OUTPUT);
    analogWrite(posPin, ZERO_SPEED);
    analogWrite(resPin, ZERO_SPEED);
}

void BiDirectionalMotor::turn(MotorState state) {
    lastState = motorState;
    if (state == motorState) {
        return;
    }
    switch (state) {
        case Positive:
            analogWrite(posPin, currentSpeed);
            analogWrite(resPin, ZERO_SPEED);
            break;
        case Reverse:
            analogWrite(resPin, currentSpeed);
            analogWrite(posPin, ZERO_SPEED);
            break;
        case Stop:
            analogWrite(posPin, ZERO_SPEED);
            analogWrite(resPin, ZERO_SPEED);
            break;
    }

    motorState = state;
}

void BiDirectionalMotor::changeSpeed(int changeTo) {
    switch (motorState) {
        case Positive:
            analogWrite(posPin, changeTo);
            break;
        case Reverse:
            analogWrite(resPin, changeTo);
            break;
        default:
            break;
    }
    currentSpeed = changeTo;
}

void BiDirectionalMotor::afterTurn() {
    turn(lastState);
}

void Car::begin() {
    leftMotor.begin();
    rightMotor.begin();
}

void Car::turnRight() {
    rightMotor.turn(isForward ? Reverse : Positive);
}

void Car::endTurnRight() {
    rightMotor.afterTurn();
}

void Car::turnLeft() {
    leftMotor.turn(isForward ? Reverse : Positive);
}

void Car::endTurnLeft() {
    leftMotor.afterTurn();
}

void Car::forward() {
    leftMotor.turn(Positive);
    rightMotor.turn(Positive);
    isForward = true;
}

void Car::backward() {
    leftMotor.turn(Reverse);
    rightMotor.turn(Reverse);
    isForward = false;
}

void Car::stop() {
    leftMotor.turn(Stop);
    rightMotor.turn(Stop);
    isForward = true;
}

void Car::goPikachu() {
    leftMotor.changeSpeed(MAX_SPEED);
    rightMotor.changeSpeed(MAX_SPEED);
}

void Car::setSpeed(int percentage) {
    int speed = (MAX_SPEED - ZERO_SPEED) * percentage / 100;
    leftMotor.changeSpeed(speed);
    rightMotor.changeSpeed(speed);
}
